/**
 * Copyright © Copyright Kevin Doyle 2012 Rhebogue Road,Limerick, Ireland.
 * Contact me at kfrdoyle@gmail.com
 * This software was created by me Kevin Doyle in 07/03/2012.
 * I made is available to the public on 07/03/2012.
 * You need my permission to use this software in a commercial context.
 * Although this code is hosted on google code I do not agree with the licensing policy
 * of google code. 
 */
package navigationfactory;

import way_point_factory.ExocetWPFactory;
import way_point_factory.WayPoint;
import way_point_factory.WayPointFactory;

/**
 *
 * @author Kevin Doyle
 */
public abstract class Navigator {
 String model, make, manufacturer;
    WayPointFactory exocetwaypointfactory = new ExocetWPFactory();
    WayPoint waypointA = exocetwaypointfactory.orderWP("Exocet", "InitialWP");
    WayPoint waypointB = exocetwaypointfactory.orderWP("Exocet", "TargetWP");

   

  

    /**
     * The calculateRange function generates range and azimuth data using
     * Vincentys' algorithm.
     *
     * @param waypoint1 stores the next sequential navigation point
     * @param waypoint2 stores the final navigation point in the flight path
     * @return s range
     */
    double calculateRange(WayPoint waypointA, WayPoint waypointB) {

        double a = 6378388;//International 1924(Europe) ellipsoid model
        double b = 6356911.946;//International 1924(Europe) ellipsoid model
        double f = 1 / 297;//International 1924(Europe) ellipsoid model
        double L = Math.toRadians(waypointA.getLongitude() - waypointB.getLongitude());
        double U1 = Math.atan((1 - f) * Math.tan(Math.toRadians(waypointB.getLatitude())));//U is reduced latitude
        double U2 = Math.atan((1 - f) * Math.tan(Math.toRadians(waypointA.getLatitude())));
        double sinU1 = Math.sin(U1);
        double cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2);
        double cosU2 = Math.cos(U2);
        double lambda = L;// difference in latitude between points
        double lambdaP;
        double iterLimit = 100;//ineration limit
        double sinLambda;
        double cosLambda;
        double sigma;
        double sinSigma;
        double cosSigma;
        double sinAlpha;
        double cosSqAlpha;
        double cos2SigmaM;
        double deltaSigma;
        double A;
        double B;
        double uSq;
        double C;
        double s;//calculated range
        do {
            sinLambda = Math.sin(lambda);
            cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt(Math.pow(cosU2 * sinLambda, 2) + Math.pow(((cosU1 * sinU2) - (sinU1 * cosU2 * cosLambda)), 2));
            if (sinSigma == 0)//reveals co incident points
            {
                System.out.println("You entered Co incident points");
            }
            cosSigma = ((sinU1 * sinU2) + (cosU1 * cosU2 * cosLambda));
            sigma = Math.atan2(sinSigma, cosSigma);
            sinAlpha = (cosU1 * (cosU2 * (sinLambda / sinSigma)));
            cosSqAlpha = 1 - Math.pow(sinAlpha, 2);
            cos2SigmaM = cosSigma - (2 * sinU1 * (sinU2 / cosSqAlpha));
            if (Double.isNaN(cos2SigmaM))//Equatoral Line cosSqAlpha=0
            {
                cos2SigmaM = 0;
            }
            C = ((f / 16) * (cosSqAlpha * ((4 + f) * ((4 - 3) * cosSqAlpha))));
            lambdaP = lambda;
            lambda = L + (1 - C) * f * sinAlpha * (sigma + (C * (sinSigma * (cos2SigmaM + (C * (cosSigma * ((-1 + 2) * Math.pow(cos2SigmaM, 2))))))));
        } while (Math.abs(lambda - lambdaP) > Math.pow(10, -12) && --iterLimit > 0);
        if (iterLimit == 0) {
            System.out.println("NaN iterLimit==0");//formula failed to converge
        }
        uSq = cosSqAlpha * ((a * a - b * b) / (b * b));
        A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * Math.pow(cos2SigmaM, 2)) - B / 6 * cos2SigmaM * (-3 + 4 * Math.pow(sinSigma, 2)) * (-3 + 4 * Math.pow(cos2SigmaM, 2))));

        s = b * A * (sigma - deltaSigma);

        return s;
    }

    /**
     * The calculateForwardAzimuth function generates range and azimuth data
     * using Vincentys' algorithm.
     *
     * @param waypointA stores the next sequential navigation point
     * @param waypointB stores the final navigation point in the flight path
     * @return fwdAz forward azimuth to next point
     */
    double calculateForwardAzimuth(WayPoint waypointA, WayPoint waypointB) {

        double a = 6378388;//International 1924(Europe) ellipsoid model
        double b = 6356911.946;//International 1924(Europe) ellipsoid model
        double f = 1 / 297;//International 1924(Europe) ellipsoid model
        double L = Math.toRadians(waypointA.getLongitude() - waypointB.getLongitude());
        double U1 = Math.atan((1 - f) * Math.tan(Math.toRadians(waypointB.getLatitude())));//U is reduced latitude
        double U2 = Math.atan((1 - f) * Math.tan(Math.toRadians(waypointA.getLatitude())));
        double sinU1 = Math.sin(U1);
        double cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2);
        double cosU2 = Math.cos(U2);
        double lambda = L;// difference in latitude between points
        double sinLambda;
        double cosLambda;
        sinLambda = Math.sin(lambda);
        cosLambda = Math.cos(lambda);

        double fwdAz = Math.atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda);
        fwdAz = Math.toDegrees(fwdAz);
        return fwdAz;
    }

    
    /**
     * The calculateNextLatitude function generates latitude and longitude
     * data using Vincentys' algorithm.
     *
     * @param waypointB stores the next sequential navigation point
     * @return nextLon Latitude
     */
      double calculateNextLatitude(WayPoint waypointB, double measurementS) {

        double a = 6378388;//International 1924(Europe) ellipsoid model
        double b = 6356911.946;//International 1924(Europe) ellipsoid model
        double f = 1 / 297;//International 1924(Europe) ellipsoid model
        double s = measurementS;//?????????????????????
        double alpha1 = Math.toRadians(3);//waypointB.getFwAzimuth ???????????
        double sinAlpha1 = Math.sin(alpha1);
        double cosAlpha1 = Math.cos(alpha1);
        double tanU1 = (1 - f) * Math.tan(Math.toRadians(waypointB.getLatitude()));
        double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1)), sinU1 = tanU1 * cosU1;
        double sigma1 = Math.atan2(tanU1, cosAlpha1);
        double sinAlpha = cosU1 * sinAlpha1;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double sigma = s / (b * A);
        double sigmaP = 2 * Math.PI;
        double cos2SigmaM = Math.cos(2 * sigma1 + sigma);
        double sinSigma = Math.sin(sigma);
        double cosSigma = Math.cos(sigma);
        while (Math.abs(sigma - sigmaP) > Math.pow(10, -12)) {
            cos2SigmaM = Math.cos(2 * sigma1 + sigma);
            sinSigma = Math.sin(sigma);
            cosSigma = Math.cos(sigma);
            double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                    - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
            sigmaP = sigma;
            sigma = s / (b * A) + deltaSigma;
        }
        double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
        double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
        
        return lat2;
    }
    
    /**
     * The calculateNextLongitude function generates latitude and longitude
     * data using Vincentys' algorithm.
     *
     * @param waypointB stores the next sequential navigation point
     * @return nextLon Longitude data
     */
    double calculateNextLongitude(WayPoint waypointB) {

        double a = 6378388;//International 1924(Europe) ellipsoid model
        double b = 6356911.946;//International 1924(Europe) ellipsoid model
        double f = 1 / 297;//International 1924(Europe) ellipsoid model
        double s = 400;//measurement; ????
        double alpha1 = Math.toRadians(waypointB.getLatitude());//?????????
        double sinAlpha1 = Math.sin(alpha1);
        double cosAlpha1 = Math.cos(alpha1);
        double tanU1 = (1 - f) * Math.tan(Math.toRadians(waypointB.getLatitude()));
        double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1)), sinU1 = tanU1 * cosU1;
        double sigma1 = Math.atan2(tanU1, cosAlpha1);
        double sinAlpha = cosU1 * sinAlpha1;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double sigma = s / (b * A);
        double sigmaP = 2 * Math.PI;
        double cos2SigmaM = Math.cos(2 * sigma1 + sigma);
        double sinSigma = Math.sin(sigma);
        double cosSigma = Math.cos(sigma);
        while (Math.abs(sigma - sigmaP) > Math.pow(10, -12)) {
            cos2SigmaM = Math.cos(2 * sigma1 + sigma);
            sinSigma = Math.sin(sigma);
            cosSigma = Math.cos(sigma);
            double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                    - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
            sigmaP = sigma;
            sigma = s / (b * A) + deltaSigma;
        }
        double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
        double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
        double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
        double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        double L = lambda - (1 - C) * f * sinAlpha
                * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
        double lon2 = (Math.toRadians(waypointB.getLongitude()) + L + 3 * Math.PI) % (2 * Math.PI) - Math.PI;  // normalise to -180...+180

        return Math.toDegrees(lon2);
    }

    public String getMake() {
        return make;
    }

    public String getManufacturer() {
        return manufacturer;
    }

    public String getModel() {
        return model;
    }

    public void setMake(String make) {
        this.make = make;
    }

    public void setManufacturer(String manufacturer) {
        this.manufacturer = manufacturer;
    }

    public void setModel(String model) {
        this.model = model;
    }
    
    
}
